
#ifndef CONTROLLER_H_
#define CONTROLLER_H_

#include "command.h"
#include "usart.h"
#include "controller_commands.h"
#include "serial_port_package.h"


template <USART_TEMPLATE>
class Controller : public UsartInterrupt
{
  public:
    Controller(void)
    {
      controller_usart.init(this);
      serial_port_receiver.setUsart(&controller_usart);
      serial_port_transmitter.setUsart(&controller_usart, false);
    }

    bool recv(Command *controller_command);
    void send(const Command &controller_command);
    void sendSensor(const Command &sensor_command);
    
    void sendOk(void);
    void work(void);
    
    // Callback from USART
    void usartInterrupt(char data);
  
  private:
    Usart<USART_TEMPLATE_PARAMS> controller_usart;
    SerialPortReceiver<USART_TEMPLATE_PARAMS> serial_port_receiver;
    SerialPortTransmitter<USART_TEMPLATE_PARAMS> serial_port_transmitter;
    SerialPortPackage serial_port_package;
    Command controller_command;
};

template <USART_TEMPLATE>
void Controller<USART_TEMPLATE_PARAMS>::usartInterrupt(char data)
{
  if (data > 250) {
    int a;
    a = 1;
  }
  //controller_usart.sendChar(data);
  serial_port_receiver.appendChar(data);
}

template <USART_TEMPLATE>
bool Controller<USART_TEMPLATE_PARAMS>::recv(Command *controller_command)
{
  if (serial_port_receiver.getPackage(&serial_port_package)) {
    controller_command->id = serial_port_package.id;
    controller_command->payload = serial_port_package.message;
    return true;
  } else {
    return false;
  }
}

template <USART_TEMPLATE>
void Controller<USART_TEMPLATE_PARAMS>::send(const Command &controller_command)
{
  serial_port_package.id = controller_command.id;
  serial_port_package.message = controller_command.payload;
  serial_port_transmitter.send(serial_port_package);
}

template <USART_TEMPLATE>
void Controller<USART_TEMPLATE_PARAMS>::sendOk(void)
{
  controller_command.id = CTRL_OK;
  controller_command.payload = "";
  //send(controller_command);
}

template <USART_TEMPLATE>
void Controller<USART_TEMPLATE_PARAMS>::work()
{
  controller_usart.work();
}

#endif /* CONTROLLER_H_ */

